// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

/* This GCS protocol sends text-based information over the GCS port
*/

#if GCS_PROTOCOL == GCS_PROTOCOL_DEBUGTERMINAL

#include <stdio.h>
 
#define DEBUGTERMINAL_VERBOSE 1

#define ERR(a) ((DEBUGTERMINAL_VERBOSE)>0?(PSTR(a)):(0))

#if GCS_PORT == 3
# define DTSTREAM	Serial3
#else
# define DTSTREAM	Serial
#endif

#define SendSer_P(a)	DTSTREAM.print_P(PSTR(a))
#define SendSerln_P(a)	DTSTREAM.println_P(PSTR(a))
#define SendSer			DTSTREAM.print
#define SendSerln		DTSTREAM.println

//Read buffer
#define DEBUGTERM_BUFFERSIZE (120)
char gcsinputbuffer[DEBUGTERM_BUFFERSIZE];
byte bufferidx;

//Reporting flags
byte report_heartbeat = 1;
byte report_attitude = 0;
byte report_location = 0;
byte report_command = 1;
byte report_severity = 0;
byte first_location = 0;

void 
readgcsinput() 
{
	byte numc, i, c;
	
	numc = DTSTREAM.available();
	for (i=0;i<numc;i++) {
		c = DTSTREAM.read();   
		processgcsinput(c);
	}
}

void 
processgcsinput(char c) 
{
	if (c==8) {
		//Received a backspace character; move the buffer index backwards
		if (bufferidx > 0) bufferidx--;
	} else if (c==10) {
		//Received a linefeed; do nothing
	} else if (c==13) {
		//Received a carriage return; process command
		gcsinputbuffer[bufferidx] = 0;
		run_debugt_command(gcsinputbuffer);
		bufferidx = 0;
	} else {
		gcsinputbuffer[bufferidx++] = c;
		if (bufferidx >= sizeof(gcsinputbuffer)) bufferidx = 0;
	}
}
		
void 
run_debugt_command(char *buf) 
{
	//*********** Ignore comments ***********
	if (strmatch(buf, PSTR("//"))) {
		
		//*********** Process 'show' commands ***********
	} else if (strmatch(buf, PSTR("show "))) {
		if (strmatch(buf+5, PSTR("heartbeat")))
			report_heartbeat = 1;
		else if (strmatch(buf+5, PSTR("attitude")))
			report_attitude = 1;
		else if (strmatch(buf+5, PSTR("location")))
			report_location = 1;
		else if (strmatch(buf+5, PSTR("command")))
			report_command = 1;
		else if (strmatch(buf+5, PSTR("severity")))
			report_severity = atoi(buf+14);
		else
			print_error(ERR("USAGE: show {heartbeat|attitude|location|command|severity <X>}"));
				
	//*********** Process 'hide' commands ***********
	} else if (strmatch(buf, PSTR("hide "))) {
		if (strmatch(buf+5, PSTR("heartbeat")))
			report_heartbeat = 0;
		else if (strmatch(buf+5, PSTR("attitude")))
			report_attitude = 0;
		else if (strmatch(buf+5, PSTR("location")))
			report_location = 0;
		else if (strmatch(buf+5, PSTR("command")))
			report_command = 0;
		else if (strmatch(buf+5, PSTR("all"))) {
			report_heartbeat = 0;
			report_attitude = 0;
			report_location = 0;
			report_command = 0;
		} else
			print_error(ERR("USAGE: hide {heartbeat|attitude|location|command|all}"));

	//*********** Process 'copy' command ***********
	} else if (strmatch(buf, PSTR("copy "))) {
		//------- copy cmd <N1> <N2> -------
		if (strmatch(buf+5, PSTR("cmd "))) {
			unsigned char i = 9, index1, index2;
			while (buf[i] != 0) {
				i++;
				if (buf[i] == ' ') break;
			}
			if (buf[i] == ' ') {
				buf[i] = 0;
				index1 = atoi(buf+9);
				index2 = atoi(buf+i+1);
				Location temp = get_wp_with_index(index1);
				set_wp_with_index(temp, index2);
				SendSer_P("Copied command index ");  SendSer(index1,DEC);  SendSer_P(" to ");  SendSerln(index2,DEC);
			} else {
				print_error(ERR("USAGE: copy cmd <srcindex> <targetindex>"));
			}
		}

	//*********** Process 'echo' command ***********
	} else if (strmatch(buf, PSTR("echo "))) {
		SendSerln(buf+5);
			
	//*********** Process 'groundstart' command ***********
	} else if (strmatch(buf, PSTR("groundstart"))) {
		startup_ground();
				
	//*********** Process 'inithome' command ***********
	} else if (strmatch(buf, PSTR("inithome"))) {
		init_home();
		SendSerln_P("Home set.");
  				
	//------- k -? -------
	} else if (strmatch(buf, PSTR("k -?"))) {
		print_error(ERR("USAGE: {print|set} {k{p|i|d}|imax} {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt}"));
		print_error(ERR("USAGE: {print|set} kff {pitchcomp|ruddermix|pitchtothrottle}"));

	//*********** Process 'print' commands ***********
	} else if (strmatch(buf, PSTR("print "))) {
		//------- print altitude -------
		if (strmatch(buf+6, PSTR("altitude"))) {
			DTSTREAM.printf_P(PSTR("Altitude:\n"
						 "  Pressure: %.2fm\n"
						 "  GPS: %.2fm\n"
						 "  Mix ratio: %.3f\n"
						 "  Mix: %.2fm\n"
						 "  Above home: %.1fm\n"),
							(float)press_alt / 100,
							(float)GPS.altitude / 100,
							altitude_mix,
							(((1.0 - altitude_mix) * GPS.altitude) + (altitude_mix * press_alt)) / 100,
							(float)get_altitude_above_home()/100);

		//------- print attitude -------
		} else if (strmatch(buf+6, PSTR("attitude"))) {
			print_attitude();

		//------- print commands[ <N1>-<N2>] -------
		} else if (strmatch(buf+6, PSTR("commands"))) {
			unsigned char dash, index1, index2;
			for (dash=14; dash<sizeof(gcsinputbuffer); dash++) {
				if (buf[dash] == 0) break;
				if (buf[dash] == '-') break;
			}
			if (buf[dash] == 0) {
				print_commands();
			} else {
				buf[dash] = 0;
				index1 = atoi(buf+14);
				index2 = atoi(buf+dash+1);
				if (index2 < index1) index2 = wp_total;
				print_commands(index1, index2);
			}

		//------- print ctrlmode -------
		} else if (strmatch(buf+6, PSTR("ctrlmode"))) {
			print_control_mode();

		//------- print curwaypts -------
		} else if (strmatch(buf+6, PSTR("curwaypts"))) {
			print_current_waypoints();

		//------- print gains -------
		} else if (strmatch(buf+6, PSTR("gains"))) {
			print_gains();

		//------- print flightmodes -------
		} else if (strmatch(buf+6, PSTR("flightmodes"))) {
			int i;
			SendSer_P("EEPROM read: ");
			for (i=0; i<6; i++) {
				SendSer_P("Ch ");  SendSer(i,DEC);  SendSer_P(" = ");  SendSer(flight_modes[i],DEC);  SendSer_P(", ");
			}
			SendSerln(" ");

		//------- print holdalt -------
		} else if (strmatch(buf+6, PSTR("holdalt"))) {
			SendSer_P("Altitude above home set to ");  SendSerln(read_alt_to_hold(),DEC);

		//------- print imax {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} -------
		} else if (strmatch(buf+6, PSTR("imax "))) {
			int i;
			unsigned char j;
			if (get_pid_offset_name(buf+11, &i, &j)) {
				SendSer_P("Integrator maximum for ");
				SendSer(buf+9);
				SendSer_P(" = ");
				SendSerln(integrator_max[i],DEC);
			} else
				print_gain_keyword_error();

		//------- print kp {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} -------
		} else if (strmatch(buf+6, PSTR("kp "))) {
			int i;
			unsigned char j;
			if (get_pid_offset_name(buf+9, &i, &j)) {
				SendSer_P("P gain for ");
				SendSer(buf+9);
				SendSer_P(" = ");
				SendSerln(kp[i],DEC);
			} else
				print_gain_keyword_error();
				
		//------- print ki {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} -------
		} else if (strmatch(buf+6, PSTR("ki "))) {
			int i;
			unsigned char j;
			if (get_pid_offset_name(buf+9, &i, &j)) {
				SendSer_P("I gain for ");
				SendSer(buf+9);
				SendSer_P(" = ");
				SendSerln(ki[i],DEC);
			} else
				print_gain_keyword_error();
				
		//------- print kd {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} -------
		} else if (strmatch(buf+6, PSTR("kd "))) {
			int i;
			unsigned char j;
			if (get_pid_offset_name(buf+9, &i, &j)) {
				SendSer_P("D gain for ");
				SendSer(buf+9);
				SendSer_P(" = ");
				SendSerln(kd[i],DEC);
			} else
				print_gain_keyword_error();
				
		//------- print kff {pitchcomp|ruddermix|pitchtothrottle} -------
		} else if (strmatch(buf+6, PSTR("kff "))) {
			if (strmatch(buf+10, PSTR("pitchcomp"))) {
				SendSer_P("FF gain for pitchcomp = ");
				SendSerln(kff[CASE_PITCH_COMP],DEC);
			} else if (strmatch(buf+10, PSTR("ruddermix"))) {
				SendSer_P("FF gain for ruddermix = ");
				SendSerln(kff[CASE_RUDDER_MIX],DEC);
			} else if (strmatch(buf+10, PSTR("pitchtothrottle"))) {
				SendSer_P("FF gain for pitchtothrottle = ");
				SendSerln(kff[CASE_P_TO_T],DEC);
			} else if (strmatch(buf+10, PSTR("throttletopitch"))) {
				SendSer_P("FF gain for throttletopitch = ");
				SendSerln(kff[CASE_T_TO_P],DEC);
			} else
				print_gain_keyword_error();

		//------- print location -------
		} else if (strmatch(buf+6, PSTR("location"))) {
			print_position();

		//------- print navrolllimit -------
		} else if (strmatch(buf+6, PSTR("navrolllimit"))) {
			SendSer_P("Nav roll limit = ");  SendSerln((float)head_max/100,2);

		//------- print navsettings -------
		} else if (strmatch(buf+6, PSTR("navsettings"))) {
			DTSTREAM.printf_P(PSTR("Navigation settings:\n"
			#if AIRSPEED_SENSOR == ENABLED
								"  Cruise airspeed: %.2f\n"
			#else
								"  Cruise throttle: %d\n"
			#endif
								"  Hold altitude above home: %ld\n"
								"  Loiter radius: %d\n"
								"  Waypoint radius: %d\n"),
			#if AIRSPEED_SENSOR == ENABLED
						   (float)airspeed_cruise / 100,
			#else
						   throttle_cruise,
			#endif
						   read_alt_to_hold(),
						   (int)loiter_radius,
						   (int)wp_radius);

		//------- print pitchmax -------
		} else if (strmatch(buf+6, PSTR("pitchmax"))) {
			SendSer_P("Maximum pitch = ");  SendSerln((float)pitch_max/100,2);

		//------- print pitchmin -------
		} else if (strmatch(buf+6, PSTR("pitchmin"))) {
			SendSer_P("Minimum pitch = ");  SendSerln((float)pitch_min/100,2);

		//------- print pitchtarget -------
		} else if (strmatch(buf+6, PSTR("pitchtarget"))) {
			SendSer_P("Target pitch = ");  SendSerln((float)pitch_target/100,2);

		//------- print pressure -------
		} else if (strmatch(buf+6, PSTR("pressure"))) {
			SendSerln_P("BMP085 pressure sensor:");
			SendSer_P("  Temperature: ");  SendSerln(APM_BMP085.Temp,DEC);
			SendSer_P("  Pressure: ");  SendSerln(APM_BMP085.Press,DEC);

		//------- print rlocation home -------
		} else if (strmatch(buf+6, PSTR("rlocation home"))) {
			print_rlocation(&home);

		//------- print rlocation -------
		//(implication is "relative to next waypoint")
		} else if (strmatch(buf+6, PSTR("rlocation"))) {
			print_rlocation(&next_WP);

		//------- print speed -------
		} else if (strmatch(buf+6, PSTR("speed"))) {
			SendSerln_P("Speed:");
			SendSer_P("  Ground: ");  SendSerln((float)GPS.ground_speed/100.0,2);
			#if AIRSPEED_SENSOR == ENABLED
				SendSer_P("  Air: ");  SendSerln((float)airspeed/100.0,2);
				SendSer_P("  Cruise: ");  SendSerln((float)airspeed_cruise/100.0,2);
			#endif

		//------- print throttlecruise -------
		} else if (strmatch(buf+6, PSTR("throttlecruise"))) {
			SendSer_P("Throttle cruise = ");  SendSer(throttle_cruise,DEC);  SendSerln_P("%");

		//------- print throttlemax -------
		} else if (strmatch(buf+6, PSTR("throttlemax"))) {
			SendSer_P("Throttle max = ");  SendSer(throttle_max,DEC);  SendSerln_P("%");

		//------- print throttlemin -------
		} else if (strmatch(buf+6, PSTR("throttlemin"))) {
			SendSer_P("Throttle min = ");  SendSer(throttle_min,DEC);  SendSerln_P("%");

		//------- print tuning -------
		} else if (strmatch(buf+6, PSTR("tuning"))) {
			print_tuning();

		} else
			print_error(ERR("USAGE: print {altitude|attitude|commands|ctrlmode|curwaypts|flightmodes|k -?|kp|ki|kd|kff|location|navsettings|pressure|rlocation [home]|speed|tuning|}"));

	//*********** Process 'reset' commands ***********
	} else if (strmatch(buf, PSTR("reset "))) {
		//------- reset commands -------
		if (strmatch(buf+6, PSTR("commands"))) {
			reload_commands();
			SendSerln_P("Commands reloaded.");
		} else
			print_error(ERR("USAGE: reset commands"));

	//*********** Process 'rtl' command ***********
	} else if (strmatch(buf, PSTR("rtl"))) {
		return_to_launch();
		SendSerln_P("Returning to launch...");

	//*********** Process 'set' commands ***********
	} else if (strmatch(buf, PSTR("set "))) {
		//------- set cmd -------
		if (strmatch(buf+4, PSTR("cmd "))) {
			process_set_cmd(buf+8, bufferidx-8);

		//------- set cmdcount -------
		} else if (strmatch(buf+4, PSTR("cmdcount "))) {
			wp_total = atoi(buf+13);
			save_EEPROM_waypoint_info();
			SendSer_P("wp_total = ");  SendSerln(wp_total,DEC);

		//------- set cmdindex -------
		} else if (strmatch(buf+4, PSTR("cmdindex "))) {
			wp_index = atoi(buf+13);
			decrement_WP_index();
			SendSer_P("Command set to index ");  SendSer(wp_index+1,DEC);
			if (next_command.id >= 0x10 && next_command.id <= 0x1F) { //TODO: create a function the defines what type of command each command ID is
				command_must_index = 0;
				SendSerln_P(" (must)");
			} else if (next_command.id >= 0x20 && next_command.id <= 0x2F) {
				command_may_index = 0;
				SendSerln_P(" (may)");
			} else
				SendSerln_P(" (now)");

			next_command.id = CMD_BLANK;
			if (wp_index > wp_total) {
				wp_total = wp_index;
				SendSer_P("  The total number of commands is now ");
				SendSerln(wp_total,DEC);
			}
			next_WP = current_loc;
			update_commands();

		//------- set cruise -------
		} else if (strmatch(buf+4, PSTR("cruise "))) {
			unsigned char j = 4+7;
			#if AIRSPEED_SENSOR == 1
				SendSer_P("airspeed_cruise changed from ");
				SendSer((float)airspeed_cruise/100,2);
				SendSer_P(" to ");
				airspeed_cruise = (int)(readfloat(buf, &j)/100000);
				airspeed_cruise = constrain(airspeed_cruise,0,9000); //TODO: constrain minimum as stall speed, maximum as Vne
				SendSerln(((float)airspeed_cruise)/100,2);
			#else
				SendSer_P("throttle_cruise changed from ");
				SendSer(throttle_cruise,DEC);
				SendSer_P(" to ");
				throttle_cruise = constrain((int)(readfloat(buf, &j)/10000000),0,200);
				SendSerln(throttle_cruise,DEC);
			#endif
			save_user_configs();

		//------- set holdalt -------
		} else if (strmatch(buf+4, PSTR("holdalt "))) {
			int tempalt = atoi(buf+12)*100;
			save_alt_to_hold((int32_t)tempalt);
			SendSerln_P("Hold altitude above home set.");
		
		//------- set imax {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} <X> -------
		} else if (strmatch(buf+4, PSTR("imax "))) {
			int i;
			unsigned char j = 0;
			if (get_pid_offset_name(buf+9, &i, &j)) {
				buf[9+j] = 0;
				SendSer_P("Integrator maximum for ");
				SendSer(buf+9);
				SendSer_P(" changed from ");
				SendSer(integrator_max[i],DEC);
				SendSer_P(" to ");
				integrator_max[i] = atoi(buf+9);
				save_EEPROM_gains();
				SendSerln(integrator_max[i],DEC);
			} else
				print_error(ERR("ERROR: Did not recognize keyword; type set k -? for more information"));

		//------- set kp {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} <X> -------
		} else if (strmatch(buf+4, PSTR("kp "))) {
			int i;
			unsigned char j = 0;
			if (get_pid_offset_name(buf+7, &i, &j)) {
				buf[7+j] = 0;
				SendSer_P("P gain for ");
				SendSer(buf+7);
				SendSer_P(" changed from ");
				SendSer(kp[i],DEC);
				SendSer_P(" to ");
				j += 7+1;
				kp[i] = ((float)readfloat(buf, &j))/10000000;
				save_EEPROM_gains();
				SendSerln(kp[i],DEC);
			} else
				print_gain_keyword_error();
				
		//------- set ki {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} <X> -------
		} else if (strmatch(buf+4, PSTR("ki "))) {
			int i;
			unsigned char j = 0;
			if (get_pid_offset_name(buf+7, &i, &j)) {
				buf[7+j] = 0;
				SendSer_P("I gain for ");
				SendSer(buf+7);
				SendSer_P(" changed from ");
				SendSer(ki[i],DEC);
				SendSer_P(" to ");
				j += 7+1;
				ki[i] = ((float)readfloat(buf, &j))/10000000;
				save_EEPROM_gains();
				SendSerln(ki[i],DEC);
			} else
				print_gain_keyword_error();
				
		//------- set kd {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} <X> -------
		} else if (strmatch(buf+4, PSTR("kd "))) {
			int i;
			unsigned char j = 0;
			if (get_pid_offset_name(buf+7, &i, &j)) {
				buf[7+j] = 0;
				SendSer_P("D gain for ");
				SendSer(buf+7);
				SendSer_P(" changed from ");
				SendSer(kd[i],DEC);
				SendSer_P(" to ");
				j += 7+1;
				kd[i] = ((float)readfloat(buf, &j))/10000000;
				save_EEPROM_gains();
				SendSerln(kd[i],DEC);
			} else
				print_gain_keyword_error();

		//------- set kff {pitchcomp|ruddermix|pitchtothrottle} <X> -------
		} else if (strmatch(buf+4, PSTR("kff "))) {
			unsigned char j = 0;
			if (strmatch(buf+8, PSTR("pitchcomp"))) {
				buf[8+9] = 0;
				SendSer_P("FF gain for ");
				SendSer(buf+8);
				SendSer_P(" changed from ");
				SendSer(kff[CASE_PITCH_COMP],DEC);
				SendSer_P(" to ");
				j = 8+9+1;
				kff[CASE_PITCH_COMP] = ((float)readfloat(buf, &j))/10000000;
				save_EEPROM_gains();
				SendSerln(kff[CASE_PITCH_COMP],DEC);
			} else if (strmatch(buf+8, PSTR("ruddermix"))) {
				buf[8+9] = 0;
				SendSer_P("FF gain for ");
				SendSer(buf+8);
				SendSer_P(" changed from ");
				SendSer(kff[CASE_RUDDER_MIX],DEC);
				SendSer_P(" to ");
				j = 8+9+1;
				kff[CASE_RUDDER_MIX] = ((float)readfloat(buf, &j))/10000000;
				save_EEPROM_gains();
				SendSerln(kff[CASE_RUDDER_MIX],DEC);
			} else if (strmatch(buf+8, PSTR("pitchtothrottle"))) {
				buf[8+15] = 0;
				SendSer_P("FF gain for ");
				SendSer(buf+8);
				SendSer_P(" changed from ");
				SendSer(kff[CASE_P_TO_T],DEC);
				SendSer_P(" to ");
				j = 8+15+1;
				kff[CASE_P_TO_T] = ((float)readfloat(buf, &j))/10000000;
				save_EEPROM_gains();
				SendSerln(kff[CASE_P_TO_T],DEC);
			} else if (strmatch(buf+8, PSTR("throttletopitch"))) {
				buf[8+15] = 0;
				SendSer_P("FF gain for ");
				SendSer(buf+8);
				SendSer_P(" changed from ");
				SendSer(kff[CASE_T_TO_P],DEC);
				SendSer_P(" to ");
				j = 8+15+1;
				kff[CASE_T_TO_P] = ((float)readfloat(buf, &j))/10000000;
				save_EEPROM_gains();
				SendSerln(kff[CASE_T_TO_P],DEC);
			} else
				print_gain_keyword_error();

		//------- set loiterradius -------
		} else if (strmatch(buf+4, PSTR("loiterradius "))) {
			loiter_radius = atoi(buf+17);
			save_EEPROM_waypoint_info();
			SendSer_P("Set loiter radius to ");  SendSer(loiter_radius,DEC);  SendSerln_P(" meters");

		//------- set navrolllimit <X> -------
		} else if (strmatch(buf+4, PSTR("navrolllimit "))) {
			unsigned char j = 17;
			SendSer_P("Nav roll limit changed from ");  SendSer((float)head_max/100,2);
			SendSer_P(" to ");
			head_max = readfloat(buf, &j)/100000;
			SendSerln((float)head_max/100,2);
			save_EEPROM_gains();

		//------- set pitchmax <X> -------
		} else if (strmatch(buf+4, PSTR("pitchmax "))) {
			unsigned char j = 13;
			SendSer_P("Maximum pitch changed from ");  SendSer((float)pitch_max/100,2);
			SendSer_P(" to ");
			pitch_max = readfloat(buf, &j)/100000;
			SendSerln((float)pitch_max/100,2);
			save_EEPROM_gains();

		//------- set pitchmin <X> -------
		} else if (strmatch(buf+4, PSTR("pitchmin "))) {
			unsigned char j = 13;
			SendSer_P("Minimum pitch changed from ");  SendSer((float)pitch_min/100,2);
			SendSer_P(" to ");
			pitch_min = readfloat(buf, &j)/100000;
			SendSerln((float)pitch_min/100,2);
			save_EEPROM_gains();

		//------- set pitchtarget <X> -------
		} else if (strmatch(buf+4, PSTR("pitchtarget "))) {
			unsigned char j = 16;
			SendSer_P("Pitch target changed from ");  SendSer((float)pitch_target/100,2);
			SendSer_P(" to ");
			pitch_target = readfloat(buf, &j)/100000;
			SendSerln((float)pitch_target/100,2);
			save_EEPROM_gains();

		//------- set rcin -------
		} else if (strmatch(buf+4, PSTR("rcin"))) {
			unsigned char index = buf[8]-'1';
			if (index < 8) {
				radio_in[index] = atoi(buf+9);
			} else
				print_error(ERR("USAGE: set rcin<N> <X>"));

		//------- set rcout -------
		} else if (strmatch(buf+4, PSTR("rcout"))) {
			unsigned char index = buf[9]-'1';
			if (index < 8) {
				radio_out[index] = atoi(buf+10);
				APM_RC.OutputCh(index, radio_out[index]);
			} else
				print_error(ERR("USAGE: set rcout<N> <X>"));

		//------- set relay -------
		} else if (strmatch(buf+4, PSTR("relay "))) {
			unsigned char newvalue = atoi(buf+10);
			if (newvalue == 0) {
				relay_off();
				SendSerln_P("Relay turned off");
			} else if (newvalue == 1) {
				relay_on();
				SendSerln_P("Relay turned on");
			} else {
				relay_toggle();
				SendSerln_P("Relay toggled");
			}

		//------- set throttlecruise <X> -------
		} else if (strmatch(buf+4, PSTR("throttlecruise "))) {
			SendSer_P("Throttle cruise changed from ");  SendSer(throttle_cruise,DEC);  SendSer_P("% to ");
			throttle_cruise = atoi(buf+19);
			SendSer(throttle_cruise,DEC);  SendSerln_P("%");
			save_user_configs();

		//------- set throttlefailsafe <N> -------
		} else if (strmatch(buf+4, PSTR("throttlefailsafe "))) {
			throttle_failsafe_value = atoi(buf+21);
			if (throttle_failsafe_value == 0)
				throttle_failsafe_enabled = 0;
			else
				throttle_failsafe_enabled = 1;
			save_user_configs();
			SendSerln_P("Throttle failsafe adjusted.");

		//------- set throttlefailsafeaction <N> -------
		} else if (strmatch(buf+4, PSTR("throttlefailsafeaction "))) {
			throttle_failsafe_action = atoi(buf+27);
			save_user_configs();

		//------- set throttlemax <X> -------
		} else if (strmatch(buf+4, PSTR("throttlemax "))) {
			SendSer_P("Throttle max changed from ");  SendSer(throttle_max,DEC);  SendSer_P("% to ");
			throttle_max = atoi(buf+16);
			SendSer(throttle_max,DEC);  SendSerln_P("%");
			save_user_configs();

		//------- set throttlemin <X> -------
		} else if (strmatch(buf+4, PSTR("throttlemin "))) {
			SendSer_P("Throttle min changed from ");  SendSer(throttle_min,DEC);  SendSer_P("% to ");
			throttle_min = atoi(buf+16);
			SendSer(throttle_min,DEC);  SendSerln_P("%");
			save_user_configs();

		//------- set wpradius -------
		} else if (strmatch(buf+4, PSTR("wpradius "))) {
			wp_radius = atoi(buf+13);
			save_EEPROM_waypoint_info();
			SendSer_P("Set waypoint radius to ");  SendSer(wp_radius,DEC);  SendSerln_P(" meters");

		//------- set xtrackentryangle -------
		} else if (strmatch(buf+4, PSTR("xtrackentryangle "))) {
			unsigned char j = 21;
			x_track_angle = readfloat(buf, &j)/100000;
			save_EEPROM_gains();
			SendSer_P("Crosstrack entry angle set to ");  SendSerln((float)x_track_angle/100,2);

		//------- set xtrackgain -------
		} else if (strmatch(buf+4, PSTR("xtrackgain "))) {
			unsigned char j = 15;
			x_track_gain = ((float)readfloat(buf, &j))/10000000;
			save_EEPROM_gains();
			SendSer_P("Crosstrack gain set to ");  SendSerln(x_track_gain,2);
			
		} else
			print_error(ERR("USAGE: set {cmd|cmdcount|cmdindex|cruise|holdalt|kp|ki|kd|kff|loiterradius|rcin|rcout|wpradius}"));

	//*********** Process 'status' commands ***********
	} else if (strmatch(buf, PSTR("status "))) {
		//------- status altitude -------
		if (strmatch(buf+7, PSTR("altitude"))) {
			DTSTREAM.printf_P(PSTR("Altitude:\n"
								"  altitude_error = %.2fm\n"
								"  target_altitude = %.2fm\n"
								"  next_WP.alt = %.2fm\n"
								"  wp_distance = %ldm\n"
								"  wp_totalDistance = %ldm\n"
								"  offset_altitude = %.2fm\n"),
								(float)altitude_error/100,
								(float)target_altitude/100,
								(float)next_WP.alt/100,
								wp_distance,
								wp_totalDistance,
								(float)offset_altitude/100);

		//------- status control -------
		} else if (strmatch(buf+7, PSTR("control"))) {
			DTSTREAM.printf_P(PSTR("Control status:\n"
								"  Roll: nav= %.2f, servo_out= %.2f\n"	// XXX float?
								"  Pitch: nav= %.2f, servo_out= %.2f\n"	// XXX float?
								"  Throttle: nav= %d, servo_out= %d\n"),
						   (float)nav_roll  / 100, (float)servo_out[CH_ROLL]  / 100,
						   (float)nav_pitch / 100, (float)servo_out[CH_PITCH] / 100,
						   throttle_cruise, servo_out[CH_THROTTLE]);								

		//------- status gps -------
		} else if (strmatch(buf+7, PSTR("gps"))) {
			DTSTREAM.printf_P(PSTR("GPS status:\n"
								"  Fix: %S (%d)\n"
								"  Satellites: %d\n"
								"  Fix count: %d\n"),
						   (GPS.fix ? PSTR("Valid") : PSTR("INVALID")),
						   (int)GPS.fix,
						   (int)GPS.num_sats,
						   gps_fix_count);

		//------- status landing -------
		} else if (strmatch(buf+7, PSTR("landing"))) {
			DTSTREAM.printf_P(PSTR("Landing status:"
								"  land_complete = %d\n"
								"  landing_pitch = %d\n"
								"  nav_pitch = %ld\n"
								"  airspeed_cruise = %d\n"
								"  throttle_cruise = %d\n"
								"  hold_course = %ld\n"
								"  nav_bearing = %ld\n"
								"  bearing_error = %ld\n"),
						   (int)land_complete,
						   landing_pitch,
						   nav_pitch,
						   airspeed_cruise,
						   throttle_cruise,
						   hold_course,
						   nav_bearing,
						   bearing_error);
			
		//------- status loiter -------
		} else if (strmatch(buf+7, PSTR("loiter"))) {
			DTSTREAM.printf_P(PSTR("Loiter status:"
								"  Loiter radius: %d\n"
								"  Progress: %d/%d\n"	// XXX original had non-ASCII units char
								"  Time: %ldms/%dms\n"),
						   (int)loiter_radius,
						   loiter_sum, loiter_total,
						   loiter_time, loiter_time_max);

		//------- status navigation -------
		} else if (strmatch(buf+7, PSTR("navigation"))) {
			DTSTREAM.printf_P(PSTR("Navigation status:\n"
								"  From <%.6f, %.6f, %.1f>: "),
								(float)prev_WP.lat/10000000.0,
								(float)prev_WP.lng/10000000.0,
								(float)prev_WP.alt/100.0);
			print_rlocation(&prev_WP);
			DTSTREAM.printf_P(PSTR("  At <%.6f, %.6f, %.1f>\n"),
								(float)current_loc.lat/10000000.0,
								(float)current_loc.lng/10000000.0,
								(float)current_loc.alt/100.0);
			DTSTREAM.printf_P(PSTR("  To <%.6f, %.6f, %.1f>: "),
								(float)next_WP.lat/10000000.0,
								(float)next_WP.lng/10000000.0,
								(float)next_WP.alt/100.0);
			print_rlocation(&next_WP);
			DTSTREAM.printf_P(PSTR("  Distance: %.1f%% %ldm / %ldm; %.1f vertically\n"),
								100.0*(float)(wp_totalDistance-wp_distance)/(float)wp_totalDistance,
								wp_totalDistance-wp_distance,
								wp_totalDistance,
								(float)altitude_error/100.0);
			DTSTREAM.printf_P(PSTR("  Nav bearing: %.2f; error = %.2f\n"),
								(float)nav_bearing/100.0,
								(float)bearing_error/100.0);
			DTSTREAM.printf_P(PSTR("  Ground course: %.1f (current), %.1f (target)\n"),
								(float)GPS.ground_course/100.0,
								(float)target_bearing/100.0);
			if (hold_course >= 0) {
				SendSer_P("  HOLD COURSE: ");  SendSerln(hold_course/100.0,2);
			}

		//------- status navpitch -------
		} else if (strmatch(buf+7, PSTR("navpitch"))) {
			#if AIRSPEED_SENSOR == ENABLED
			DTSTREAM.printf_P(PSTR(">>> nav_pitch = PID[airspeed_error (%.2f) = airspeed_cruise (%.2f) - airspeed (%.2f)]\n"),
						   (float)airspeed_error / 100,
						   (float)airspeed_cruise / 100,
						   (float)airspeed / 100);
			#else
			DTSTREAM.printf_P(PSTR(">>> nav_pitch = PID[altitude_error (%ld) = target_altitude (%ld) - current_loc.alt (%ld)]\n"),
						   altitude_error,
						   target_altitude,
						   current_loc.alt);
			#endif
			DTSTREAM.printf_P(PSTR("nav_pitch (%.2f) - pitch_sensor (%.2f) + pitch_comp (%.2f) = %.2f\n"),
						   (float)nav_pitch / 100,
						   (float)pitch_sensor / 100,
						   abs(roll_sensor * kff[CASE_PITCH_COMP]) / 100,
						   (float)(nav_pitch-pitch_sensor + abs(roll_sensor * kff[CASE_PITCH_COMP])) / 100);
			DTSTREAM.printf_P(PSTR("servo_out[CH_PITCH] (%.2f) = PID[nav_pitch + pitch_comp - pitch_sensor]"),
						   (float)servo_out[CH_PITCH] / 100);

		//------- status navroll -------
		} else if (strmatch(buf+7, PSTR("navroll"))) {
			print_rlocation(&next_WP);
			DTSTREAM.printf_P(PSTR("bearing_error (%ld) = nav_bearing (%ld) - yaw_sensor (%ld)\n"
								"nav_roll (%ld) - roll_sensor (%ld) = %ld\n"
								"servo_out[CH_ROLL] = %d\n"),
						   bearing_error, nav_bearing, yaw_sensor,
						   nav_roll, roll_sensor, nav_roll - roll_sensor,
						   servo_out[CH_ROLL]);

		//------- status pid {servoroll|servopitch|servorudder|navroll|navpitchasp|navpitchalt|throttlete|throttlealt} -------
		} else if (strmatch(buf+7, PSTR("pid "))) {
			int i;
			unsigned char j;
			if (get_pid_offset_name(buf+11, &i, &j)) {
				SendSer(buf+11);
				SendSer_P(": ");
				display_PID = i; //The next time a PID calculation is performed on this channel, the print_PID routine in this GCS will be called
			} else
				print_error(ERR("ERROR: Did not recognize keyword"));

		//------- status rcinputch -------
		} else if (strmatch(buf+7, PSTR("rcinputch"))) {
			unsigned char i;
			SendSerln_P("RC hardware input:");
			for (i=0; i<8; i++) {
				SendSer_P("  Ch");
				SendSer(i+1,DEC);
				SendSer_P(": ");
				SendSerln(APM_RC.InputCh(i));
			}

		//------- status rcin -------
		} else if (strmatch(buf+7, PSTR("rcin"))) {
			unsigned char i;
			SendSerln_P("RC software input:");
			for (i=0; i<8; i++) {
				SendSer_P("  Ch");
				SendSer(i+1,DEC);
				SendSer_P(": ");
				SendSerln(radio_in[i]);
			}

		//------- status rcout -------
		} else if (strmatch(buf+7, PSTR("rcout"))) {
			unsigned char i;
			SendSerln_P("RC software output:");
			for (i=0; i<8; i++) {
				SendSer_P("  Ch");
				SendSer(i+1,DEC);
				SendSer_P(": ");
				SendSerln(radio_out[i]);
			}

		//------- status rcpwm -------
		} else if (strmatch(buf+7, PSTR("rcpwm"))) {
			unsigned char i;
			SendSerln_P("RC hardware output:");
			for (i=0; i<8; i++) {
				SendSer_P("  Ch");
				SendSer(i+1,DEC);
				SendSer_P(": ");
				SendSerln(readOutputCh(i));
			}

		//------- status rctrim -------
		} else if (strmatch(buf+7, PSTR("rctrim"))) {
			unsigned char i;
			SendSerln_P("RC trim:");
			for (i=0; i<8; i++) {
				SendSer_P("  Ch");
				SendSer(i+1,DEC);
				SendSer_P(": ");
				SendSerln(radio_trim[i]);
			}
			SendSer_P("  elevon1_trim = ");  SendSerln(elevon1_trim,DEC);
			SendSer_P("  elevon2_trim = ");  SendSerln(elevon2_trim,DEC);

		//------- status rc -------
		} else if (strmatch(buf+7, PSTR("rc"))) {
			unsigned char i;
			SendSerln_P("RC:\tCh\tHWin\tSWtrim\tSWin\tServo\tSWout\tHWout\t");
			for (i=0; i<8; i++) {
				// XXX might benefit from field widths, since some terminals tab badly
				DTSTREAM.printf_P(PSTR("\t%u\t%u\t%u\t%u\t%.2f\t%d\t%d\n"),
							   (unsigned int)(i + 1),
							   APM_RC.InputCh(i),
							   radio_trim[i],
							   radio_in[i],
							   (float)servo_out[i] / 100,
							   radio_out[i],
							   readOutputCh(i));
			}

		//------- status system -------
		} else if (strmatch(buf+7, PSTR("system"))) {
			DTSTREAM.printf_P(PSTR("System status:"
								"  Ground start: %S (%d)\n"
								"  Home: %SSET (%d)\n"),
						   (ground_start ? PSTR("YES") : PSTR("NO")), (int)ground_start,
						   (home_is_set  ? PSTR("") : PSTR("NOT ")), (int)home_is_set);

		//------- status takeoff -------
		} else if (strmatch(buf+7, PSTR("takeoff"))) {
			SendSerln_P("Takeoff status:");
			SendSer_P("  takeoff in progress: ");
			if (!takeoff_complete)
				SendSerln_P("YES");
			else {
				SendSer_P("NO");
			}
			DTSTREAM.printf_P(PSTR("  takeoff_pitch = %.2f\n"
								"  scaler = %.3f\n"
								"  nav_pitch = %.2f\n"
								"  throttle = %d\n"
								"  hold_course = %.2f\n"
								"  nav_bearing = %ld\n"
								"  bearing_error = %ld\n"
								"  current_loc.alt = %ld\n"
								"  home.alt + takeoff_altitude = %ld"),
						   (float)takeoff_pitch / 100,
						   (float)airspeed / (float)airspeed_cruise,
						   (float)nav_pitch / 100,
						   servo_out[CH_THROTTLE],
						   (float)hold_course / 100,
						   nav_bearing,
						   bearing_error,
						   current_loc.alt,
						   home.alt + takeoff_altitude);
			
		//------- status telemetry -------
		} else if (strmatch(buf+7, PSTR("telemetry"))) {
			DTSTREAM.printf_P(PSTR("Telemetry status:\n"
								"  %S heartbeat\n"
								"  %S location\n"
								"  %S attitude\n"
								"  %S command\n"
								"  Severity report level %d\n"),
						   (report_heartbeat ? PSTR("Show") : PSTR("Hide")),
						   (report_location  ? PSTR("Show") : PSTR("Hide")),
						   (report_attitude  ? PSTR("Show") : PSTR("Hide")),
						   (report_command   ? PSTR("Show") : PSTR("Hide")),
						   (int)report_severity);

		//------- status throttle -------
		} else if (strmatch(buf+7, PSTR("throttle"))) {
#if AIRSPEED_SENSOR == ENABLED
			DTSTREAM.printf_P(PSTR(">>> airspeed_energy_error (%ld) = 0.5 * (airspeed_cruise (%.2f)^2 - airspeed (%.2f)^2)\n"
								"energy_error (%ld) = airspeed_energy_error (%ld) + altitude_error*0.098 (%ld)\n"
								"servo_out[CH_THROTTLE] (%d) = PID[energy_error]\n"),
						   airspeed_energy_error, (float)airspeed_cruise / 100, (float)airspeed / 100,
						   energy_error, airspeed_energy_error, (long)((float)altitude_error * 0.098),
						   servo_out[CH_THROTTLE]*100);
#else
			DTSTREAM.printf_P(PSTR("altitude_error (%ld) = target_altitude (%ld) - current_loc.alt (%ld)\n"
								"servo_out[CH_THROTTLE] (%d) = PID[altitude_error]\n"),
						   altitude_error, target_altitude, current_loc.alt,
						   servo_out[CH_THROTTLE]*100);
#endif

		//------- status waypoints -------
		} else if (strmatch(buf+7, PSTR("waypoints"))) {
			DTSTREAM.printf_P(PSTR("Waypoints status:\n"
								"  Distance: %ld/%ld\n"
								"  Index: %d/%d\n"
								"  Next: %d\n"),
						   wp_distance, wp_totalDistance,
						   (int)wp_index, (int)wp_total,
						   (int)next_wp_index);
			
		} else if (strmatch(buf+7, PSTR("james"))) {
			SendSerln_P("James is a monkey");
		} else {
			print_error(ERR("USAGE: status {control|gps|landing|loiter|mixing|navigation|navpitch|navroll|rc|rcinputch|rcin|rcout|rcpwm|rctrim|system|takeoff|telemetry|throttle|waypoints}"));
		}

	} else {
		print_error(ERR("USAGE: {echo <text>|groundstart|inithome|show|hide|print|status|set|reset|rtl}"));
		print_error(ERR("Type <command> -? for specific usage guidance"));
	}
}

/* strmatch compares two strings and returns 1 if they match and 0 if they don't
   s2 must be stored in program memory (via PSTR) rather than RAM (like standard strings)
   s1 must be at least as long as s2 for a valid match
   if s1 is longer than s2, then only the beginning of s1 (the length of s2) must match s2 for a valid match */
int
strmatch(char* s1, const char* s2)
{
	int i = 0;
	char c1 = s1[0], c2 = pgm_read_byte(s2);

	while (c1 != 0 && c2 != 0) {
		if (c1 < c2)
			return 0;
		if (c1 > c2)
			return 0;
		i++;
		c1 = s1[i];
		c2 = pgm_read_byte(s2+i);
	}

	if (c2==0)
		return 1;
	else
		return 0;
}

/* readfloat parses a string written as a float starting at the offset in *i and updates *i as it parses
   numbers are multiplied by 10,000,000 and decimals beyond 7 places are discarded
   parsing is terminated with either a space or a null, other characters are ignored */
long
readfloat(char* s, unsigned char* i) 
{
	long result = 0, multiplier = 0;
	char c, neg = 0;
	
	for (;;(*i)++) {
		c = s[*i];
		if (c == ' ')
			break;
		else if (c == 0)
			break;
		else if (c == '-')
			neg = 1-neg;
		else if (c == '.') {
			result *= 10000000;
			multiplier = 1000000;
		} else if (c >= '0' && c <= '9') {
			if (multiplier == 0)
				result = (result*10) + c-'0';
			else {
				result += (c-'0')*multiplier;
				multiplier /= 10;
			}
		}
	}

	if (multiplier == 0)
		result *= 10000000;
	
	if (neg)
		return -result;
	else
		return result;
}

/* process_set_cmd processing the parameters of a 'set cmd' command and takes the appropriate action
   *buffer is the buffer containing the parameters of the command; it should start after the space after 'set cmd'
   bufferlen is the length of the buffer; the routine will stop looking for parameters after the offset index reaches this value
*/
#define SETPARAM_NONE (0)
#define SETPARAM_ID (1)
#define SETPARAM_P1 (2)
#define SETPARAM_LAT (3)
#define SETPARAM_LNG (4)
#define SETPARAM_ALT (5)
#define SETPARAM_P2 (6)
#define SETPARAM_P3 (7)
#define SETPARAM_P4 (8)

void 
process_set_cmd(char *buffer, int bufferlen) 
{
	unsigned char i, j, err=1, setparam=SETPARAM_NONE;
	unsigned char cmdindex=0, p1=0, cmdid;
	long lat, lng, alt;
	Location temp;

	//Parse the command index
	for (i=0; i<bufferlen; i++)
		if (buffer[i] >= '0' && buffer[i] <= '9')
			cmdindex = (cmdindex*10) + buffer[i]-'0';
		else
			break;
	
	if (buffer[i] == ' ') {
		//Find the end of the command-type string
		i++;
		for (j=i; j<bufferlen; j++) {
			if (buffer[j] == ' ' || buffer[j] == 0)
				break;
		}
		if (buffer[j] == ' ') {
			//Process the command-type string
			buffer[j] = 0; //Null-terminate the command-type string so strmatch can figure out when to stop comparing, and so atoi can work
			if (strmatch(buffer+i, PSTR("waypoint")))
				cmdid = CMD_WAYPOINT;
			else if (strmatch(buffer+i, PSTR("takeoff")))
				cmdid = CMD_TAKEOFF;
			else if (strmatch(buffer+i, PSTR("landoptions")))
				cmdid = CMD_LAND_OPTIONS;
			else if (strmatch(buffer+i, PSTR("land")))
				cmdid = CMD_LAND;
			else if (strmatch(buffer+i, PSTR("loiternturns")))
				cmdid = CMD_LOITER_N_TURNS;
			else if (strmatch(buffer+i, PSTR("loitertime")))
				cmdid = CMD_LOITER_TIME;
			else if (strmatch(buffer+i, PSTR("loiter")))
				cmdid = CMD_LOITER;
			else if (strmatch(buffer+i, PSTR("delay")))
				cmdid = CMD_DELAY;
			else if (strmatch(buffer+i, PSTR("resetindex")))
				cmdid = CMD_RESET_INDEX;
			else if (strmatch(buffer+i, PSTR("throttlecruise")))
				cmdid = CMD_THROTTLE_CRUISE;
			else if (strmatch(buffer+i, PSTR("resethome")))
				cmdid = CMD_RESET_HOME;
			else if (strmatch(buffer+i, PSTR("index")))
				cmdid = CMD_INDEX;
			else if (strmatch(buffer+i, PSTR("rtl")))
				cmdid = CMD_RTL;
			else if (strmatch(buffer+i, PSTR("relay")))
				cmdid = CMD_RELAY;
			else if (strmatch(buffer+i, PSTR("servo")))
				cmdid = CMD_SERVO;
			else if (strmatch(buffer+i, PSTR("id")))
				setparam = SETPARAM_ID;
			else if (strmatch(buffer+i, PSTR("p1")))
				setparam = SETPARAM_P1;
			else if (strmatch(buffer+i, PSTR("alt")))
				setparam = SETPARAM_ALT;
			else if (strmatch(buffer+i, PSTR("lat")))
				setparam = SETPARAM_LAT;
			else if (strmatch(buffer+i, PSTR("lng")))
				setparam = SETPARAM_LNG;
			else if (strmatch(buffer+i, PSTR("p2")))
				setparam = SETPARAM_P2;
			else if (strmatch(buffer+i, PSTR("p3")))
				setparam = SETPARAM_P3;
			else if (strmatch(buffer+i, PSTR("p4")))
				setparam = SETPARAM_P4;
			else
				cmdid = atoi(buffer+i);
			
			if (setparam > SETPARAM_NONE) {
				//Process new parameter value
				i = j+1;
				if (setparam == SETPARAM_ALT || setparam == SETPARAM_LAT || setparam == SETPARAM_LNG) {
					lat = readfloat(buffer, &i);
				} else {
					unsigned char k;
					for (k=i; i<sizeof(gcsinputbuffer); k++)
						if (buffer[k] != '-' && !(buffer[k] >= '0' && buffer[k] <= '9'))
							break;
					buffer[k] = 0;
					lat = atol(buffer+i);
					i = k;
				}

				temp = get_wp_with_index(cmdindex);
				if (setparam == SETPARAM_ID)
					temp.id = lat;
				else if (setparam == SETPARAM_P1)
					temp.p1 = lat;
				else if (setparam == SETPARAM_ALT)
					temp.alt = lat/100000;
				else if (setparam == SETPARAM_LAT)
					temp.lat = lat;
				else if (setparam == SETPARAM_LNG)
					temp.lng = lat;
				else if (setparam == SETPARAM_P2)
					temp.alt = lat;
				else if (setparam == SETPARAM_P3)
					temp.lat = lat;
				else if (setparam == SETPARAM_P4)
					temp.lng = lat;
				cmdid = temp.id;
				p1 = temp.p1;
				lat = temp.lat;
				lng = temp.lng;
				alt = temp.alt;
				err = 0;
			} else {
				//Process param 1
				for (i=j+1; i<bufferlen; i++) {
					if (buffer[i] >= '0' && buffer[i] <= '9')
						p1 = (p1*10) + buffer[i]-'0';
					else
						break;
				}

				if (buffer[i] == ' ') {
					//Process altitude
					i++;
					if (strmatch(buffer+i, PSTR("here"))) {
						lat = GPS.latitude;
						lng = GPS.longitude;
						alt = get_altitude_above_home(); //GPS.altitude;
						err = 0;
					} else {
						alt = readfloat(buffer, &i)/100000;

						if (buffer[i] == ' ') {
							//Process latitude
							i++;
							lat = readfloat(buffer, &i);
							if (strmatch(buffer+i, PSTR("here"))) {
								lat = GPS.latitude;
								lng = GPS.longitude;
								err = 0;
							} else {
								if (buffer[i] == ' ') {
									//Process longitude
									i++;
									lng = readfloat(buffer, &i);

									//TODO: add other command special cases here
									if (cmdid == CMD_LAND_OPTIONS) {
										temp.p1 = p1;
										temp.alt = alt;
										temp.lat = lat / 10000000;
										temp.lng = lng / 10000000;
									}

									err = 0;
								} else
									print_error(ERR("Error processing set cmd: Could not find longitude parameter"));
							}
						} else
							print_error(ERR("Error processing set cmd: Could not find latitude parameter"));
					}
				} else
					print_error(ERR("Error processing set cmd: Could not find altitude parameter"));
			}
		} else
			print_error(ERR("Error processing set cmd: Could not find command type"));
	} else
		print_error(ERR("Error processing set cmd: Could not find command index"));
		
	if (err == 0) {
		temp.id = cmdid;
		temp.p1 = p1;
		temp.lat = lat;
		temp.lng = lng;
		temp.alt = alt;

		if (cmdindex >= wp_total) {
			wp_total = cmdindex+1;
			save_EEPROM_waypoint_info();
		}
		set_wp_with_index(temp, cmdindex);

		DTSTREAM.printf_P(PSTR("Set command %d to %d with p1=%d, lat=%ld, lng=%ld, alt=%ld\n"),
					   (int)cmdindex, (int)temp.id, (int)temp.p1, temp.lat, temp.lng, temp.alt);
	}
}

/* get_pid_offset_name matches a string expressed in *buffer with a pid keyword and returns the k-array
   gain offset in *offset, and the length of that string expression in *length.  If a good keyword
   match is found, 1 is returned; 0 otherwise
*/
int
get_pid_offset_name(char *buffer, int *offset, unsigned char *length) 
{
	if (strmatch(buffer, PSTR("servoroll"))) {
		*length += 9;  *offset = CASE_SERVO_ROLL;
	} else if (strmatch(buffer, PSTR("servopitch"))) {
		*length += 10;  *offset = CASE_SERVO_PITCH;
	} else if (strmatch(buffer, PSTR("servorudder"))) {
		*length += 11;  *offset = CASE_SERVO_RUDDER;
	} else if (strmatch(buffer, PSTR("navroll"))) {
		*length += 7;  *offset = CASE_NAV_ROLL;
	} else if (strmatch(buffer, PSTR("navpitchasp"))) {
		*length += 11;  *offset = CASE_NAV_PITCH_ASP;
	} else if (strmatch(buffer, PSTR("navpitchalt"))) {
		*length += 11;  *offset = CASE_NAV_PITCH_ALT;
	} else if (strmatch(buffer, PSTR("throttlete"))) {
		*length += 10;  *offset = CASE_TE_THROTTLE;
	} else if (strmatch(buffer, PSTR("throttlealt"))) {
		*length += 11;  *offset = CASE_ALT_THROTTLE;
	} else {
		return 0;
	}
	
	return 1;
}


/* print_rlocation prints the relative location of the specified waypoint from the plane in easy-to-read cartesian format
*/
void
print_rlocation(Location *wp) 
{
	//float x, y;
	//y = (float)(wp->lat - current_loc.lat) * 0.0111194927;
	//x = (float)(wp->lng - current_loc.lng) * cos(radians(current_loc.lat)) * 0.0111194927;
	int x, y;
	y = (wp->lat - current_loc.lat) * 0.0111194927;
	x = (wp->lng - current_loc.lng) * cos(radians(current_loc.lat)) * 0.0111194927;
	DTSTREAM.printf_P(PSTR("dP = <%d%c, %d%c, %.1f> meters\n"),
				   abs(y), (y >= 0 ? 'N' : 'S'),
				   abs(x), (x >= 0 ? 'E' : 'W'),
				   (float)(wp->alt - current_loc.alt) / 100);
}


/* print_error prints an error message if the user sends an invalid command
*/
void
print_error(const char *msg) 
{
	if (msg == 0)
		SendSerln_P("ERROR: Invalid command");
	else
		DTSTREAM.println_P(msg);
}


void
send_message(unsigned char severity, const char *str)
{
	if(severity >= DEBUG_LEVEL){
		SendSer_P("MSG: ");
		SendSerln(str);
	}
}

void
send_message(unsigned char id)
{
	send_message(id, 0l);
}

void
send_message(unsigned char id, long param)
{
	switch(id) {
		case MSG_HEARTBEAT:
			if (report_heartbeat)
				print_control_mode();
			break;
		
		case MSG_ATTITUDE:
			if (report_attitude)
				print_attitude();
			break;
			
		case MSG_LOCATION:
			if (report_location)
				print_position();
			if (first_location == 0) {
				send_message((uint8_t)0,PSTR("First location received"));
				first_location = 1;
			}
			break;
		
		case MSG_COMMAND:
			struct Location cmd = get_wp_with_index(param);
			print_command(&cmd, param);
			break;
		
	}
}
			
void
print_current_waypoints()
{
	DTSTREAM.printf_P(PSTR("Current waypoints:"
						 "  Prev:\t%ld,\t%ld\t%ld\n"
						 "  Cur: \t%ld,\t%ld\t%ld\n"
						 "  Next:%d\t%ld,\t%ld\t%ld\n"),
					prev_WP.lat, prev_WP.lng, prev_WP.alt,	
					current_loc.lat, current_loc.lng, current_loc.alt,
					(int)wp_index, next_WP.lat, next_WP.lng, next_WP.alt);
}

void
print_position(void)
{
	DTSTREAM.printf_P(PSTR("Pos: %ld, %ld, %ldcm, %ldcm/s GS, %d cm/s AS, %d cm above home, %d? climb, %ldm from wp, %d%% throttle, %ld alt err\n"),
					current_loc.lat, current_loc.lng, current_loc.alt,
					GPS.ground_speed,
					airspeed,
					get_altitude_above_home(),
					climb_rate,
					wp_distance,
					throttle_cruise,
					altitude_error);	
}

void
print_attitude(void)
{
	DTSTREAM.printf_P(PSTR("Att: %u roll_in, %u pitch in, %u throttle_in, "
						 "%ld roll_sensor, %ld pitch_sensor, "
						 "%ld ground_course, %ld target_bearing, "
						 "%ld nav_roll, %d loiter_sum, "
						 "%d roll servo_out, %d pitch_servo_out\n"),
					radio_in[CH_ROLL], radio_in[CH_PITCH], radio_in[CH_THROTTLE],
					roll_sensor, pitch_sensor,
					GPS.ground_course, target_bearing,
					nav_roll, loiter_sum,
					servo_out[CH_ROLL], servo_out[CH_PITCH]);
}

// required by Groundstation to plot lateral tracking course 
void
print_new_wp_info()
{
	DTSTREAM.printf_P(PSTR("New waypt (%d): %ld, %ld, %ldm -> "
						 "%ld, %ld, %ldm; "
						 "%ldm dist, %u roll trim, %u pitch trim\n"),
					(int)wp_index,
					prev_WP.lat, prev_WP.lng, prev_WP.alt,
					next_WP.lat, next_WP.lng, next_WP.alt,
					wp_totalDistance,
					radio_trim[CH_ROLL], radio_trim[CH_PITCH]);
}

void
print_control_mode(void)
{
	switch (control_mode){
		case MANUAL:
			SendSerln_P("##MANUAL");
			break;
		case STABILIZE:
			SendSerln_P("##STABILIZE");
			break;
		case FLY_BY_WIRE_A:
			SendSerln_P("##FBW A");
			break;
		case FLY_BY_WIRE_B:
			SendSerln_P("##FBW B");
			break;
		case AUTO:
			SendSerln_P("##AUTO");
			break;
		case RTL:
			SendSerln_P("##RTL");
			break;
		case LOITER:
			SendSerln_P("##LOITER");
			break;
		case 98:
			SendSerln_P("##Air Start Complete");
			break;
		case 99:
			SendSerln_P("##Ground Start Complete");
			break;
	}
}

void
print_tuning(void) 
{
	DTSTREAM.printf_P(PSTR("TUN:%d,    %ld,     %ld,    %d,    %ld,    %ld\n"),
					servo_out[CH_ROLL],  nav_roll  / 100, roll_sensor  / 100,
					servo_out[CH_PITCH], nav_pitch / 100, pitch_sensor / 100);					
}


void
print_command_id(byte id)
{
	switch (id) {
		// Command IDs - Must
		case CMD_BLANK: SendSer_P("CMD_BLANK");  break;
		case CMD_WAYPOINT: SendSer_P("waypoint");  break;
		case CMD_LOITER: SendSer_P("loiter");  break;
		case CMD_LOITER_N_TURNS: SendSer_P("loiternturns");  break;
		case CMD_LOITER_TIME: SendSer_P("loitertime");  break;
		case CMD_RTL: SendSer_P("rtl");  break;
		case CMD_LAND: SendSer_P("land");  break;
		case CMD_TAKEOFF: SendSer_P("takeoff");  break;

		// Command IDs - May
		case CMD_DELAY: SendSer_P("delay");  break;
		case CMD_CLIMB: SendSer_P("climb");  break;
		case CMD_LAND_OPTIONS: SendSer_P("landoptions");  break;

		// Command IDs - Now
		case CMD_RESET_INDEX: SendSer_P("resetindex");  break;
		case CMD_GOTO_INDEX: SendSer_P("CMD_GOTO_INDEX");  break;
		case CMD_GETVAR_INDEX: SendSer_P("CMD_GETVAR_INDEX");  break;
		case CMD_SENDVAR_INDEX: SendSer_P("CMD_SENDVAR_INDEX");  break;
		case CMD_TELEMETRY: SendSer_P("CMD_TELEMETRY");  break;

		case CMD_THROTTLE_CRUISE: SendSer_P("throttlecruise");  break;
		case CMD_AIRSPEED_CRUISE: SendSer_P("CMD_AIRSPEED_CRUISE");  break;
		case CMD_RESET_HOME: SendSer_P("resethome");  break;

		case CMD_KP_GAIN: SendSer_P("CMD_KP_GAIN");  break;
		case CMD_KI_GAIN: SendSer_P("CMD_KI_GAIN");  break;
		case CMD_KD_GAIN: SendSer_P("CMD_KD_GAIN");  break;
		case CMD_KI_MAX: SendSer_P("CMD_KI_MAX");  break;
		case CMD_KFF_GAIN: SendSer_P("CMD_KFF_GAIN");  break;

		case CMD_RADIO_TRIM: SendSer_P("CMD_RADIO_TRIM");  break;
		case CMD_RADIO_MAX: SendSer_P("CMD_RADIO_MAX");  break;
		case CMD_RADIO_MIN: SendSer_P("CMD_RADIO_MIN");  break;
		case CMD_ELEVON_TRIM: SendSer_P("CMD_ELEVON_TRIM");  break;

		case CMD_INDEX: SendSer_P("index");  break;
		case CMD_REPEAT: SendSer_P("CMD_REPEAT");  break;
		case CMD_RELAY: SendSer_P("relay");  break;
		case CMD_SERVO: SendSer_P("servo");  break;

		default: SendSer(id,DEC);
	}
}

void
print_waypoint(struct Location *cmd, byte index)
{
	print_command(cmd, index);
}

void
print_command(struct Location *cmd,byte index)
{
	SendSer_P("    command #: ");
	SendSer(index, DEC);
	SendSer_P(" id: ");
	print_command_id(cmd->id);

	SendSer_P(" p1: ");
	SendSer(cmd->p1,DEC);
	SendSer_P(" p2/alt: ");
	SendSer(cmd->alt,DEC);
	SendSer_P(" p3/lat: ");
	SendSer(cmd->lat,DEC);
	SendSer_P(" p4/lng: ");
	SendSerln(cmd->lng,DEC);
}

void
print_commands()
{
	print_commands(1, wp_total);
}

void
print_commands(unsigned char i1, unsigned char i2)
{
	SendSerln_P("Commands in memory:");
	SendSer_P("  ");
	SendSer(wp_total, DEC);
	SendSerln_P(" commands total");
	// create a location struct to hold the temp Waypoints for printing
	//Location tmp;
	SendSerln_P("  Home: ");
	struct Location cmd = get_wp_with_index(0);
	print_command(&cmd, 0);
	SendSerln_P("  Commands: ");
	
	for (int i=i1; i<=i2; i++){
		cmd = get_wp_with_index(i);
		print_command(&cmd, i);
		delay(10);
	}
}

void
print_gains()
{
	unsigned char i;

	SendSerln_P("PID gains   \tP    \tI    \tD    \tIMax)");
	SendSer_P("Servo roll  \t");  print_gain(CASE_SERVO_ROLL);
	SendSer_P("Servo pitch \t");  print_gain(CASE_SERVO_PITCH);
	SendSer_P("Servo yaw   \t");  print_gain(CASE_SERVO_RUDDER);
	SendSer_P("Nav roll    \t");  print_gain(CASE_NAV_ROLL);

	SendSer_P("Nav pitch   \t");
	if (AIRSPEED_SENSOR)
		print_gain(CASE_NAV_PITCH_ASP);
	else
		print_gain(CASE_NAV_PITCH_ALT);

	SendSer_P("Nav throttle\t");
	if (AIRSPEED_SENSOR)
		print_gain(CASE_TE_THROTTLE);
	else
		print_gain(CASE_ALT_THROTTLE);

	SendSerln_P("Feed-forward gains");
	SendSer_P("Pitch compensation\t");  SendSerln(kff[CASE_PITCH_COMP],3);
	SendSer_P("Rudder mix        \t");  SendSerln(kff[CASE_RUDDER_MIX],3);
	SendSer_P("Pitch to throttle \t");  SendSerln(kff[CASE_P_TO_T],3);
	SendSer_P("Throttle to pitch \t");  SendSerln(kff[CASE_T_TO_P],3);
}

void
print_gain(unsigned char g)
{
	SendSer(kp[g],3);
	SendSer_P("\t");
	SendSer(ki[g],3);
	SendSer_P("\t");
	SendSer(kd[g],3);
	SendSer_P("\t");
	SendSerln(integrator_max[g],DEC);
}

void
print_gain_keyword_error()
{
	print_error(ERR("ERROR: Did not recognize keyword; type k -? for more information"));
}

void
print_PID(long PID_error, long dt, float scaler, float derivative)
{
	SendSer_P("P = ");			SendSer(kp[display_PID] * scaler * (float)PID_error,2);
	SendSer_P(",\tI = ");		SendSer(integrator[display_PID],2);
	SendSer_P(",\tD = ");		SendSer(kd[display_PID] * scaler * derivative,2);
	SendSer_P("\terrors = {");	SendSer(PID_error,DEC);
	SendSer_P(", ");				SendSer(last_error[display_PID],DEC);
	SendSer_P("} with dt = ");	SendSerln(dt,DEC);

	display_PID = -1;
}

#endif // GCS_PROTOCOL_DEBUGTERMINAL
